#include "trajectory.h"
#include <assert.h>
#include <malloc.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include "func_trapz.h"

void trajectory_calc(const union trajectory * tr, double t, double * v, enum trajectory_calc_task task){
	switch(tr -> id){
		case TRAJ_LINE:{
			const double * r0 = tr -> line . r0;
			const double * r1 = tr -> line . r1;
			switch(task){
				case TRAJECTORY_CALC_R:
				{
					// r = (r1 - r0)*t + r0
					int ksi;
					for(ksi=0;ksi<3;ksi+=1){
						v [ksi] = (r1[ksi] - r0[ksi])*t + r0[ksi];
					}
				}
				break;
				case TRAJECTORY_CALC_DR_DT:
				{
					// dr/dt = r1-r0
					int ksi;
					for(ksi=0;ksi<3;ksi+=1){
						v [ksi] = r1[ksi] - r0[ksi];
					}
				}
				break;
				default:
					assert(FALSE);
			}
		}
		break;
		case TRAJ_BEZIER2:{
			const double * P0 = tr -> bezier2.P0;
			const double * P1 = tr -> bezier2.P1;
			const double * P2 = tr -> bezier2.P2;
			switch(task){
				case TRAJECTORY_CALC_R:
				{
					// r = P0*(1-t)^2 + 2*P1*t*(1-t)+P2*t^2
					int ksi;
					for(ksi=0;ksi<3;ksi+=1){
						v [ksi] = P0[ksi]*pow(1-t,2) + 2*P1[ksi]*t*(1-t) + P2[ksi]*pow(t,2);
					}
				}
				break;
				case TRAJECTORY_CALC_DR_DT:
				{
					int ksi;
					for(ksi=0;ksi<3;ksi+=1){
						v [ksi] = 2*t*(P0[ksi]-2*P1[ksi]+P2[ksi]) + 2*(P1[ksi]-P0[ksi]);
					}
				}
				break;
				default:
					assert(FALSE);
			}
		}
		break;
		case TRAJ_BEZIER3:{
			const double * P0 = tr -> bezier3.P0;
			const double * P1 = tr -> bezier3.P1;
			const double * P2 = tr -> bezier3.P2;
			const double * P3 = tr -> bezier3.P3;
			switch(task){
				case TRAJECTORY_CALC_R:{
					int ksi;
					for(ksi = 0; ksi < 3; ksi += 1){
						v[ksi] = P0[ksi]*pow((1-t),3) + 
							3*P1[ksi]*pow((1-t),2)*t + 
							3*P2[ksi]*(1-t)*pow(t,2) + 
							P3[ksi]*pow(t,3);
					}
				}
				break;
				case TRAJECTORY_CALC_DR_DT:{
					int ksi;
					for(ksi = 0; ksi < 3; ksi += 1){
						v[ksi] = -3*P0[ksi]*pow(1-t,2) + 
							3*P1[ksi]*(1-t)*(1-3*t) + 
							3*P2[ksi]*t*(2-3*t) + 
							3*P3[ksi]*pow(t,2);
					}
				}
				break;
				default:
					assert(FALSE);
			}
		}
		break;
		default:
			assert(FALSE);
	}
}


/**
 * Implements a function that calculates \frac{dl}{dt} for trajectories
 * that need numeric integration to determine dependency between t and 
 * length. The function definition matches that needed for func_trapz
 * integrator.
 * @param t - trajectory parameter \in [0,1]
 * @param tr - pointer to 'union trajectory' object
 * @return dl/dt at specified t, same to \mathbb{R}^{3} derrivative norm
 */
static complex trajectory_dl_dt(double t, void * tr){
	union trajectory * traj = tr;
	complex dl_dt;
	double dr_dt[3];
	trajectory_calc(traj, t, dr_dt, TRAJECTORY_CALC_DR_DT);
	dl_dt = gsl_hypot3(dr_dt[0], dr_dt[1], dr_dt[2]);
	return dl_dt;
}



int trajectory_len_create(union trajectory_len * len, union trajectory * tr){
	double eps = 1e-5;// length function mapping precision
	switch(tr -> id){
		case TRAJ_LINE:{
			double * r0 = tr -> line . r0;
			double * r1 = tr -> line . r1;
			len -> id = TRAJECTORY_LEN_LINEAR;
			len -> linear . kappa = gsl_hypot3(r0[0]-r1[0], r0[1]-r1[1], r0[2]-r1[2]);;
		}
		break;
		case TRAJ_BEZIER2:
		case TRAJ_BEZIER3:{//All trajectories that need numeric integration
			struct func_trapz_stats stats;
			complex result;
			complex * result_arr;
			int trapz_status = func_trapz(trajectory_dl_dt, tr, 0, 1, eps, &result, &result_arr, &stats);
			//It uses complex for function value, but for the sake of reuse let's integrate double values with it
			if(trapz_status){
				return 2;
			}
			double * yy = malloc(sizeof(double) * stats.nknot);
			double * xx  = malloc(sizeof(double) * stats.nknot);
			if( xx == NULL || yy == NULL ){
				free(xx);
				free(yy);
				free(result_arr);
				return 1;
			}
			{// Filling in xx and yy
				uint32_t k;
				double sum = 0;
				yy[0]=0;
				xx[0]=0;
				for(k=1;k<stats.nknot;k+=1){
					sum += creal(result_arr[k-1]);
					yy[k] = sum;
					xx[k] = k / (double) (stats.nknot - 1);
				}
			}
			free(result_arr);// No more needed
			// Constructing cspline by data produced.
			uint8_t init_stat;
			len -> numeric . cs = cspline_init( & init_stat, xx, yy, stats.nknot);
			if(NULL == len -> numeric . cs){
				// Failed to allocate
				free(xx);
				free(yy);
				return (int)100 + init_stat;
			}
			len -> id = TRAJECTORY_LEN_NUMERIC;
			// xx and yy are now in responsibility of cspline object
		}
		break;
		default:
			assert(FALSE);
	}
	return 0;
}


double trajectory_len_calc(const union trajectory_len * len, double t){
	double ans = GSL_NAN;
	switch(len -> id){
		case TRAJECTORY_LEN_LINEAR:{
			ans = len -> linear.kappa * t;
		}
		break;
		case TRAJECTORY_LEN_NUMERIC:{
			assert(!
				cspline_calculate(len -> numeric.cs, t, & ans)
			);
		}
		break;
		default:
			assert(FALSE);
	}
	return ans;
}



void trajectory_len_free(union trajectory_len * len){
	switch(len -> id){ // Those, that don't need freeing, are skipped
		case TRAJECTORY_LEN_NUMERIC:{
			cspline_free(len -> numeric.cs);
		}
		break;
	}
}



double trajectory_max_dist(const union trajectory * tr, const double * r){
	double ans;
	switch(tr -> id){
		case TRAJ_LINE:{
			/* Max @ trajectory edges for LINE */
			uint8_t ksi;
			double r0[3], r1[3], R0, R1;
			trajectory_calc(tr, 0.0, r0, TRAJECTORY_CALC_R);
			trajectory_calc(tr, 1.0, r1, TRAJECTORY_CALC_R);
			for(ksi=0;ksi<3;ksi+=1){
				r0[ksi] -= r[ksi]; 
				r1[ksi] -= r[ksi];
				// Vector difference is now in r{0,1}
			}
			R0 = gsl_hypot3(r0[0], r0[1], r0[2]);
			R1 = gsl_hypot3(r1[0], r1[1], r1[2]);
			// returning maximum
			if( R0 > R1 ){
				ans = R0;
			}else{
				ans = R1;
			}
			//Done
		}
		break;
		case TRAJ_BEZIER2:
		case TRAJ_BEZIER3:{
			// Unless it really needs optimisation, we'll use simple search with step decreasing
			// Unless the studied function has different scales of changing, that has more than 2^rescaling_2_for_OK
			// scale difference, this algorythm will success.
			// Fail e.g. would be for a function that has a maximum in a small local peak.
			// That's not the case with our (Normal) trajectories, however.
			uint32_t Nknot = 10; // Initial N of knots
			double max = 0;//Max of the last iteration
			
			// Numeric method parameters
			double eps = 0.01; // Maximum detection precision
			uint8_t maxlogsize = 5; // If max doesn't differ from that in queue for more than eps, OK - we finish the algorythm
			// 2^maxlogsize scale difference lies between the iterations
			uint8_t o; // variable to iterate through maxlog
			double maxlog[maxlogsize]; // Maximums at previous iterations
			for(o=0;o<maxlogsize;o+=1){
				maxlog[o] = GSL_NAN;
			}
			uint8_t maxlog_left_to_full = maxlogsize; // Need to insert this number of records into maxlog before starting to test for exit condition
			while(TRUE){
				uint32_t ksi;
				for(ksi = 0; ksi < Nknot; ksi+=1){
					double t = (double)ksi / (Nknot - 1);
					double vr[3];
					trajectory_calc(tr, t, vr, TRAJECTORY_CALC_R);
					double R = gsl_hypot3(vr[0] - r[0], vr[1] - r[1], vr[2] - r[2]);
					if(R > max){
						max = R;
					}
				}
				// Found maximum
				
				if(maxlog_left_to_full){
					maxlog_left_to_full -= 1;
				}else{
					// Ready to check break condition
					double maxreldiff = 0;
					boolean fail = FALSE;
					for(o = 0; o < maxlogsize; o += 1){
						double reldiff = fabs((max - maxlog[o]) / max);
						if (reldiff > eps){
							fail = TRUE;
							break;
						}
					}
					if(!fail){
						break; // Ready!
					}
				}
				for(o = maxlogsize - maxlog_left_to_full - 1; o > 0; o-=1){
					maxlog[o] = maxlog[o-1];
				}
				maxlog[0] = max;
				
				// Make step smaller
				uint32_t newN = Nknot * 2;
				assert(newN > Nknot); // Nknot overflow, quite unrealistic, however
				Nknot = newN;
			}
			// max of the last iteration doesn't differ from the prevous much
			ans = max*(1+eps); // To compensate error a bit :P
		}
		break;
		default:
			assert(FALSE);
	}
	return ans;
}








int trajectory_len_copy(const union trajectory_len * src, union trajectory_len * dst){
	dst -> id = src -> id;
	switch(src -> id){
		case TRAJECTORY_LEN_LINEAR:{
			dst -> linear.kappa = src -> linear.kappa;
		}
		break;
		case TRAJECTORY_LEN_NUMERIC:{
			dst -> id = src -> id;
			dst -> numeric.cs = src -> numeric.cs;
			// Informating length object that it's now referenced by one more object
			cspline_notify_copied(dst -> numeric.cs);
		}
		break;
		default:
			assert(FALSE);
	}
	return 0;
}

/**
 * Reflects v to vr over z=0 plane.
 * @param v - vector to reflect
 * @param vr - it's reflection
 */
static void rflvz0(const double * v, double * vr) {
	vr[0] = v[0]; vr[1] = v[1]; vr[2] = -v[2]; 
}

void trajectory_mirror(const union trajectory * src, union trajectory * refl){
	refl -> id = src -> id;//Copy type
	switch(src -> id){
		case TRAJ_LINE:{
			rflvz0(src -> line.r0, refl -> line.r0);
			rflvz0(src -> line.r1, refl -> line.r1);
		}
		break;
		case TRAJ_BEZIER2:{
			rflvz0(src -> bezier2.P0, refl -> bezier2.P0);
			rflvz0(src -> bezier2.P1, refl -> bezier2.P1);
			rflvz0(src -> bezier2.P2, refl -> bezier2.P2);
		}
		break;
		case TRAJ_BEZIER3:{
			rflvz0(src -> bezier3.P0, refl -> bezier3.P0);
			rflvz0(src -> bezier3.P1, refl -> bezier3.P1);
			rflvz0(src -> bezier3.P2, refl -> bezier3.P2);
			rflvz0(src -> bezier3.P3, refl -> bezier3.P3);
		}
		break;
		default:
			assert(FALSE);
	}
}






